﻿
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using BaseDll;
using System.Threading;
using System.Reflection;
namespace MotionIoLib
{
    public enum ScaraAlarm
    {
        四轴机器人气缸到位失败 = 206,
        四轴机器人运动到位超时,
    }

    public abstract class BaseScaraRobot
    {
        public BaseScaraRobot(string strName, params object[] RobotParam)
        {

        }
        public string strName = "";
        public Dictionary<string, int> dic_IoIn = new Dictionary<string, int>();
        public Dictionary<string, int> dic_IoOut = new Dictionary<string, int>();
        public abstract bool Open();
        public abstract bool Close();
        public abstract bool InPos(Coordinate coord);
        public abstract bool IsConnect();
        public abstract bool IsServoOn();

        public abstract bool IsPowerOn();
        public abstract bool GetInBit(int index);
        public abstract bool GetOutBit(int index);
        public abstract Coordinate GetCurrentCoordinate();
        public abstract HandDirection GetCurrentHandDirection();
        public abstract void ServoOn(bool bServo);
        public abstract void PowerOn(bool bHighPower);

        public abstract void ResetRobot();
        public abstract void SetVel(double d_Vel);
        public abstract void SetAcc(double d_Acc);
        public abstract bool Jump(Coordinate coordinate, double limz, bool bCheckHandleSys = false);
        public abstract bool JumpInpos(Coordinate coordinate, bool bmanual, double limz, bool bCheckHandleSys = false, int n_TimeOut = 3000);
        public abstract bool Go(Coordinate coordinate,  bool bCheckHandleSys = false);
        public abstract bool GoInpos(Coordinate coordinate, bool b_manual, bool bCheckHandleSys = false,int n_TimeOut=30000);
        public abstract bool CheckIoByName( string strIoName, bool b_State, int n_TimeOut = 3000, int n_FilterTime = 20);
        public abstract bool Move(Coordinate coordinate, bool bCheckHandleSys);
        public abstract bool MoveInpos(Coordinate coordinate, bool bmanual, bool bCheckHandleSys, int n_TimeOut = 3000);

        public abstract void Stop();
        public abstract bool SetStopActionFlag();
        public abstract bool ResetStopActionFlag();
        public abstract bool Home();
        public abstract void SetOutput(string strIoName, bool value);
        public abstract bool ReadOutput(string strIoName);
        public abstract bool ReadInput(string strIoName);
        public abstract bool CheckRLSystem(HandDirection handDirection);

        public abstract bool RepeatLink();
    }

    public class RobotMotionMgr
    {
        Thread thread = null;
        private Dictionary<string, BaseScaraRobot> dicRobot = new Dictionary<string, BaseScaraRobot>();
        private static RobotMotionMgr _RobotObj = null;
        private static object _lock = new object();
        public delegate bool IsSafeWhenRobotMoveHandler(Coordinate coord_Current, Coordinate coord_Tagret);
        public event IsSafeWhenRobotMoveHandler m_eventIsSafeWheRobotMove = null;
        public static RobotMotionMgr GetInstace()
        {

            if (_RobotObj == null)
            {
                lock (_lock)
                {
                    if (_RobotObj == null)
                    {
                        _RobotObj = new RobotMotionMgr();
                        return _RobotObj;
                    }
                    else
                        return _RobotObj;
                }
            }
            else
                return _RobotObj;
        }

        public void AddRobot(string strName, string strType, params object[] RobotParam)
        {
            Assembly assembly = Assembly.GetAssembly(typeof(BaseScaraRobot));
            string name = "MotionIoLib.Robot_" + strType;
            //Motion_Advantech
            Type type = assembly.GetType(name);
            bool flag = type == null;
            if (flag)
            {
                throw new Exception(string.Format($"机器人：{strName} 配置失败，请检查参数"));
            }
            object[] args = new object[RobotParam.Length];
            for (int i = 0; i < RobotParam.Length; i++)
            {
                args[i] = RobotParam[i];
            }

            if (!dicRobot.ContainsKey(strName))
            {
                dicRobot.Add(strName, Activator.CreateInstance(type, strName, args) as BaseScaraRobot);
            }
        }
        public BaseScaraRobot GetRobot(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName];
            }
            return null;
        }
        public bool InitRobotSystem()
        {
            foreach (var item in Enum.GetValues(typeof(ScaraAlarm)))
            {
                CommonDlg.AlarmInfoMannage.GetInstance().AddAlarmInfo((int)item, item.ToString());

            }

            foreach (var item in dicRobot)
            {
                if(!item.Value.Open())
                    return false;
            }
            thread = new Thread(ThreadMonitor);
            thread.IsBackground = true;
            thread.Start();
            return true;
        }
        public void CloseRobotSystem()
        {
            b_Exit = true;

            Thread.Sleep(500);
            thread.Join(500);
            try
            {
                if (thread.IsAlive)
                    thread.Abort();
            }
            catch (Exception)
            {

            }
            foreach (var item in dicRobot)
            {
                item.Value.Close();
            }
        }
        public bool GetInBit(string strName,string strIoName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                if(dicRobot[strName].dic_IoIn.ContainsKey(strIoName))
                   return dicRobot[strName].GetInBit(dicRobot[strName].dic_IoIn[strIoName]);
            }
            return false;
        }
        public bool GetOutBit(string strName, string strIoName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                if (dicRobot[strName].dic_IoOut.ContainsKey(strIoName))
                    return dicRobot[strName].GetOutBit(dicRobot[strName].dic_IoOut[strIoName]);
            }
            return false;
        }
        public Coordinate GetCurrentCoordinate(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].GetCurrentCoordinate();
            }
            else
            {
                return new Coordinate() { X = -9999, Y = -9999, Z = -9999, U = -9999, };
            }
        }
        public void ServoOn(string strName,bool bServo)
        {
            if (dicRobot.ContainsKey(strName))
            {
                 dicRobot[strName].ServoOn(bServo);
            }
        }
        public void ResetRobot(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                dicRobot[strName].ResetRobot();
            }
        }
        public void SetVel(string strName,double d_Vel)
        {
            if (dicRobot.ContainsKey(strName))
            {
                dicRobot[strName].SetVel(d_Vel);
            }
        }
        public void SetAcc(string strName,double d_Acc)
        {
            if (dicRobot.ContainsKey(strName))
            {
                dicRobot[strName].SetAcc(d_Acc);
            }
        }
        public bool Jump(string strName,Coordinate coordinate, double limz, bool bCheckHandleSys = false)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].Jump(coordinate, limz, bCheckHandleSys);
            }
            else
            {
                return false;
            }
        }
        public bool Go(string strName,Coordinate coordinate, bool bCheckHandleSys = false)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].Go(coordinate, bCheckHandleSys);
            }
            else
            {
                return false;
            }
        }
        public bool Move(string strName,Coordinate coordinate, bool bCheckHandleSys)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].Move(coordinate, bCheckHandleSys);
            }
            else
            {
                return false;
            }
        }
        public void Stop(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                dicRobot[strName].Stop();
            }

        }
        public bool SetStopActionFlag(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].SetStopActionFlag();
            }
            else
            {
                return false;
            }

        }
        public bool ResetStopActionFlag(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].ResetStopActionFlag();
            }
            else
            {
                return false;
            }
        }
        public bool Home(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].Home();
            }
            else
            {
                return false;
            }
        }
        public void SetOutput(string strName,string strIoName, bool value)
        {
            if (dicRobot.ContainsKey(strName))
            {
                if (dicRobot[strName].dic_IoOut.ContainsKey(strIoName))
                    dicRobot[strName].SetOutput(strIoName, value);
            }
        }
        public bool CheckRLSystem(string strName,HandDirection handDirection)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].CheckRLSystem(handDirection);
            }
            else
                return false;
        }

        public bool RepeatLink(string strName)
        {
            if (dicRobot.ContainsKey(strName))
            {
                return dicRobot[strName].RepeatLink();
            }
            else
                return false;
        }

        public bool IsSafeFunRegister(IsSafeWhenRobotMoveHandler isSafeWhenAxisMoveHandler)
        {
            if (m_eventIsSafeWheRobotMove != null)
            {
                Delegate[] delegates = m_eventIsSafeWheRobotMove.GetInvocationList();
                if (delegates != null && delegates.Length > 0)
                {
                    for (int i = 0; i < delegates.Length; i++)
                    {
                        if (delegates[i] == isSafeWhenAxisMoveHandler.GetInvocationList()[0])
                            return true;
                    }
                }
            }
            return false;
        }

        public bool RobotMoveSafeCheck( Coordinate coord_Current,Coordinate coord_Tagert)
        {
            bool bSafe = true;
            if (m_eventIsSafeWheRobotMove != null)
            {
                Delegate[] delegates = m_eventIsSafeWheRobotMove.GetInvocationList();
                foreach (var tem in delegates)
                {
                    MethodInfo methodInfo = tem.GetMethodInfo();
                    bSafe = bSafe & (bool)methodInfo.Invoke(null, new object[] { coord_Current, coord_Tagert });
                }
            }
            return bSafe;
        }

        public delegate void Event_ConnectState(string strRobotName,  bool b_Connect);
        public event Event_ConnectState m_eventConnectState = null;
        public delegate void Event_ServoState(string strRobotName,  bool b_Servo);
        public event Event_ServoState m_eventServoState = null;
        public delegate void Event_PowerState(string strRobotName,  bool b_HighPower);
        public event Event_PowerState m_eventPowerState = null;
        public delegate void Event_ChangePos(string strRobotName, Coordinate coord);
        public event Event_ChangePos m_eventChangePos = null;
        public delegate void Event_ChangeHandDirection(string strRobotName,  HandDirection direction);
        public event Event_ChangeHandDirection m_eventChangeHandDirection = null;
        public delegate void IoInputStateChangeHandler(string strRobotName, string str_Name, bool currentState);
        public event IoInputStateChangeHandler m_eventIoInputChanage;
        public delegate void IoOutputStateChangeHandler(string strRobotName, string str_Name, bool currentState);
        public event IoOutputStateChangeHandler m_eventIoOutChanage;
        private bool b_Exit = false;
        public Dictionary<string, Dictionary<string, bool>> dic_Input = new Dictionary<string, Dictionary<string, bool>>();
        public Dictionary<string, Dictionary<string, bool>> dic_Output = new Dictionary<string, Dictionary<string, bool>>();
        private Dictionary<string, HandDirection> dic_HandDirection = new Dictionary<string, HandDirection>();
        private Dictionary<string, Coordinate> dic_Coordinate = new Dictionary<string, Coordinate>();
        private Dictionary<string, bool> dic_ConnectState = new Dictionary<string, bool>();
        private Dictionary<string, bool> dic_ServoState = new Dictionary<string, bool>();
        private Dictionary<string, bool> dic_HighPower = new Dictionary<string, bool>();

        private void ThreadMonitor()
        {
            foreach (var item in dicRobot)
            {
                Dictionary<string, bool> dicInput = new Dictionary<string, bool>();
                Dictionary<string, bool> dicOutput = new Dictionary<string, bool>();

                foreach (var item_IoIn in item.Value.dic_IoIn)
                {
                    dicInput.Add(item_IoIn.Key, false);
                }
                foreach (var item_IoOut in item.Value.dic_IoOut)
                {
                    dicOutput.Add(item_IoOut.Key, false);
                }
                dic_Input.Add(item.Key, dicInput);
                dic_Output.Add(item.Key, dicOutput);
                dic_ServoState.Add(item.Key, false);
                dic_ConnectState.Add(item.Key, false);
                dic_HandDirection.Add(item.Key, HandDirection.None);
                dic_HighPower.Add(item.Key, false);
                dic_Coordinate.Add(item.Key, new Coordinate() { X = -9999, Y = -9999, Z = -9999, U = -9999 });
            }
            while (!b_Exit)
            {
                Thread.Sleep(1);
                foreach (var item in dicRobot)
                {
                    string strRobotName = item.Key;
                    Dictionary<string, bool> dicInput = dic_Input[strRobotName];
                    Dictionary<string, bool> dicOutput = dic_Output[strRobotName];
                    foreach (var item_IoIn in item.Value.dic_IoIn)
                    {
                        string strInputName = item_IoIn.Key;
                        int n_Index = item_IoIn.Value;
                        bool b_State = item.Value.GetInBit(n_Index);
                        if(dicInput[strInputName]!= b_State)
                        {
                            if(m_eventIoInputChanage!=null)
                                m_eventIoInputChanage(strRobotName,strInputName, b_State);
                            dicInput[strInputName] = b_State;
                        }
                    }
                    foreach (var item_IoOut in item.Value.dic_IoOut)
                    {
                        string strOutputName = item_IoOut.Key;
                        int n_Index = item_IoOut.Value;
                        bool b_State = item.Value.GetOutBit(n_Index);
                        if (dicOutput[strOutputName] != b_State)
                        {
                            if (m_eventIoOutChanage != null)
                                m_eventIoOutChanage(strRobotName,strOutputName, b_State);
                            dicOutput[strOutputName] = b_State;
                        }
                    }

                    HandDirection hand = item.Value.GetCurrentHandDirection();
                    if (dic_HandDirection[strRobotName] != hand)
                    {
                        if (m_eventChangeHandDirection != null)
                            m_eventChangeHandDirection(strRobotName, hand);
                        dic_HandDirection[strRobotName] = hand;
                    }

                    Coordinate corrdinate = item.Value.GetCurrentCoordinate();
                    Coordinate currentPosition = dic_Coordinate[strRobotName];
                    if (corrdinate.X!= currentPosition.X|| corrdinate.Y != currentPosition.Y || corrdinate.Z != currentPosition.Z || corrdinate.U != currentPosition.U)
                    {
                        if (m_eventChangePos != null)
                            m_eventChangePos(strRobotName, corrdinate);
                        dic_Coordinate[strRobotName] = corrdinate;
                    }

                    bool b_Connect = item.Value.IsConnect();
                    if (dic_ConnectState[strRobotName] != b_Connect)
                    {
                        if (m_eventConnectState != null)
                            m_eventConnectState(strRobotName, b_Connect);
                        dic_ConnectState[strRobotName] = b_Connect;
                    }
                    bool b_ServoOn = item.Value.IsServoOn();
                    if (dic_ServoState[strRobotName] != b_ServoOn)
                    {
                        if (m_eventServoState != null)
                            m_eventServoState(strRobotName, b_ServoOn);
                        dic_ServoState[strRobotName] = b_ServoOn;
                    }
                    bool b_HighPowerOn = item.Value.IsPowerOn();
                    if (dic_HighPower[strRobotName] != b_HighPowerOn)
                    {
                        if (m_eventPowerState != null)
                            m_eventPowerState(strRobotName, b_ServoOn);
                        dic_HighPower[strRobotName] = b_HighPowerOn;
                    }

                }

            }
        }

    }

}